#ifndef SPLINEVISUALIZER_H
#define SPLINEVISUALIZER_H

#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Eigen>
#include <map>

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

#include "imu_processor/estimator_trajectory_manager.h"
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>

namespace lio {

	class splineVisualizer {
	public:
		splineVisualizer(std::string frame_id, ros::NodeHandle &nh) {
			past_path.header.frame_id = frame_id;
			active_path.header.frame_id = frame_id;
			newest_path.header.frame_id = frame_id;

			past_marker.header.frame_id = frame_id;
			static_marker.header.frame_id = frame_id;
			active_marker.header.frame_id = frame_id;
			newest_marker.header.frame_id = frame_id;

			discrete_path.header.frame_id = frame_id;

			past_path_pub = nh.advertise<nav_msgs::Path>("/vis/past_path",1, true);
			active_path_pub = nh.advertise<nav_msgs::Path>("/vis/active_path",1, true);
			newest_path_pub = nh.advertise<nav_msgs::Path>("/vis/newest_path",1, true);

			past_control_pub = nh.advertise<visualization_msgs::MarkerArray>("/vis/past_control", 10);
			static_control_pub = nh.advertise<visualization_msgs::MarkerArray>("/vis/static_control", 10);
			active_control_pub = nh.advertise<visualization_msgs::MarkerArray>("/vis/active_control", 10);
			newest_control_pub = nh.advertise<visualization_msgs::MarkerArray>("/vis/newest_control", 10);

			discrete_path_pub = nh.advertise<nav_msgs::Path>("/vis/discrete_path",1, true);
		}

		void bindTrajectory(TrajectoryManager::Ptr &traj_manager);

		void showSplinePath();
		void updatePath();
		void pubPath();

		void setMarkerAttribute();
		void showSplineControl();
		void updateMarkerArray();
		void pubMarkerArray();

		void pubDiscretePath(std::vector<Eigen::Vector3d> &postions);

	public:
		typedef std::shared_ptr<splineVisualizer> Ptr;

	private:
		ros::Publisher past_path_pub, active_path_pub, newest_path_pub, discrete_path_pub;
		nav_msgs::Path past_path, active_path, newest_path, discrete_path;
		TrajectoryManager::Ptr traj_manager_;
		double last_past_time;
		double delta;  // 采样间隔

		ros::Publisher past_control_pub, static_control_pub, active_control_pub, newest_control_pub;
		visualization_msgs::Marker past_marker, static_marker, active_marker, newest_marker;
		visualization_msgs::MarkerArray past_markers, static_markers, active_markers, newest_markers;

	};

}






#endif